#include "testMotorJoystick.h"
#include "arduinomega/HardwareSerial.h"

#define DEBUG true

extern "C" void __cxa_pure_virtual(){}

void setup();
void loop();
void printMotorInput();


int main()
{
	// The init() function is defined in the Arduino library.
	// It initialises the timers and the ADC on the ATmega1280.

	init();

	JoystickMotor joystickMotor;

	joystickMotor.setup();

	while( true )
	{
		joystickMotor.loop();
		delay(5000);
		//joystickMotor.printMotorInput();
	}

	return 0;
}

void JoystickMotor::setup()
{
	//set baud rate for serial output
	Serial.begin( BAUD_RATE );

	//set pins to receive input
	pinMode(ENABLE_RIGHT_MOTOR_PIN, OUTPUT);
	pinMode(ENABLE_LEFT_MOTOR_PIN, OUTPUT);

	pinMode( SERVO_JOYSTICK_POWER_PIN, OUTPUT );
	pinMode( RIGHT_MOTOR_CONTROL_PIN_1, OUTPUT);
	pinMode( RIGHT_MOTOR_CONTROL_PIN_2, OUTPUT);
}

void JoystickMotor::loop()
{

		digitalWrite( ENABLE_LEFT_MOTOR_PIN, HIGH);
		digitalWrite( LEFT_MOTOR_CONTROL_PIN_2, LOW );
		digitalWrite( LEFT_MOTOR_CONTROL_PIN_1, HIGH );

}

void JoystickMotor::printMotorInput()
{
	//Serial.print( "r: " );
	//Serial.print( rightPotentiometerReading );
	//Serial.print("\n\r");
	//Serial.print( "l: " );
	//Serial.print( leftPotentiometerReading );
	//Serial.print("\n\r");
	Serial.print( "s: " );
	Serial.print( servoPotentiometerReading  );
	Serial.print("\n\r");
	delay( 200 );
}
